#include <Arduino.h>
/***全局***/
void urgent_stop(void);
#define SW3 4
int SW3_state = 1;

void sw3_init(void);
void interruptServiceRoutinesw3(void);
long date;

/***微动开关定义***/
#define SW1 23
#define SW2 22
int SW1_state = 1;
int SW2_state = 1;
void sw_init(void);
void interruptServiceRoutinesw1(void);
void interruptServiceRoutinesw2(void);

/***电机宏定义***/
#define motor_PWM_frq 10000
#define motor_PWM_res 8
#define M1_A 18
#define M1_B 5
#define M1_PWM 19
#define motor_PWM_channelA 1
#define M2_A 13
#define M2_B 12
#define M2_PWM 14
#define motor_PWM_channelB 2
static int pwma_vaule = 80;
static int pushspeed = 90;
static int popspeed = 90;
static int pwmb_vaule = 155;
static int big_motor_speed = 0;
static int small_motor_speed = 0;
void PWM_init();
void setdir_speedA(int dir, uint32_t speed);
void setdir_speedB(int dir, uint32_t speed);
void setsetp_speed(int dir, uint32_t setpspeed);
void set_dir(int dir);
void ashcan_push();
void ashcan_pop();
static int M1_step = 0;
static int M1_dir = 0;
static int M1_pos = 100;
static int M1_dir_pos = 100;
static int M2_pos = 100;
static int M2_act = 3;
#define PWM_SETP0 0
#define PWM_SETP1 14
#define PWM_SETP2 28
#define PWM_SETP3 42
#define PWM_SETP4 57
#define PWM_SETP5 71
#define PWM_SETP6 85
#define PWM_SETP7 100
#define ACT_PUSH 1
#define ACT_POP 0
int M1_state = 0;

/***称重模块***/
#define HX_SCK 26
#define HX_DT 27
long count0 = 0;
long count = 0;
void HX_init();
long ReadCount();
/*
    Just a little test message.  Go to http://192.168.4.1 in a web browser
   connected to this access point to see it.
*/
String rec_str;

void setup()
{
    Serial.begin(115200);
    Serial2.begin(19200);
    Serial.println();
    /* You can remove the password parameter if you want the AP to be open. */
    delay(2000);
    HX_init();
    PWM_init();
    sw_init();
    sw3_init();
    date = millis();
}

void loop()
{
    // Serial.println("loop");
    while (Serial2.available())
    {
        delay(2);
        rec_str = Serial2.readStringUntil('\n');
    }
    //处理数据
    if (rec_str.length() > 0)
    {
        rec_str.trim();
        Serial.println(rec_str);
        if (rec_str.indexOf("M1:") != -1)
        {
            M1_pos = rec_str.indexOf("M1:");
            M1_dir_pos = rec_str.indexOf("dir:");
            M1_step = rec_str.substring(M1_pos + 3, M1_pos + 4).toInt();
            // int a= rec_str.substring(M1_pos, M1_pos+1).toInt();
            Serial.print("M1:");
            Serial.println(M1_step);
            // M1_step = 0;
            Serial.print("M1:");
            Serial.println(M1_step);
            M1_dir = rec_str.substring(M1_dir_pos + 4, M1_dir_pos + 5).toInt();
            // int b= rec_str.substring(M1_dir_pos, M1_dir_pos+1).toInt();
            if (M1_step != PWM_SETP0)
            {
                Serial.println("M1_pos:" + String(M1_pos) + "--M1_dir_pos:" + String(M1_dir_pos));
                Serial.println("M1_DIR:" + String(M1_dir) + "--M1_step:" + String(M1_step));
                Serial.print("M1:");
                Serial.println(M1_step);
                //setsetp_speed(M1_dir, M1_step);
                set_dir(M1_dir);
                set_speedA(M1_step * 14);
                M1_state = 1;
            }
            else
            {
                Serial.println("M1_pos:" + String(M1_pos) + "--M1_dir_pos:" + String(M1_dir_pos));
                Serial.println("M1_DIR:" + String(M1_dir) + "--M1_step:" + String(M1_step));
                // setsetp_speed(M1_dir, 0);
                set_dir(M1_dir);
                set_speedA(0);
                M1_state = 0;
            }
            Serial.println("REC:M1_DIR:" + String(M1_dir) + "--M1_step:" + String(M1_step));
        }
        else if (rec_str.indexOf("M2:") != -1)
        {
            M2_pos = rec_str.indexOf("M2:");
            M2_act = rec_str.substring(M2_pos + 3, M2_pos + 4).toInt();
            Serial.print("M2_act:");
            Serial.println(M2_act);
            if (M2_act == ACT_PUSH && M1_state == 0)
            {
                ashcan_push();
                Serial.println("ashcan push");
            }
            else if (M2_act == ACT_POP && M1_state == 0)
            {
                ashcan_pop();
                Serial.println("ashcan pop");
            }
        }
        rec_str = "";
        Serial.println("handle once!");
    }

    // HX711数据上报 200ms上报一次
    if (millis() - date > 200)
    {
        // static int a;i
        date = millis();
        int quality;
        count = ReadCount() - count0;
        quality = count / 447;
        // quality = a++;
        if (quality >= 0)
        {
            String HX711_str;
            HX711_str = String("HX:") + String(quality) + "END";
            Serial2.println(HX711_str);
        }
        else
        {
            String HX711_str;
            HX711_str = String("HX:") + String(0) + "END";
            Serial2.println(HX711_str);
        }
        // Serial.print("HX.Send:");
        // Serial.println(HX711_str);
    }
    // SW3_state = digitalRead(SW3);
    // if (SW3_state == 0)
    // {
    //     urgent_stop();
    //     while (1)
    //     {
    //         Serial.println("uraget stop");
    //         delay(200);
    //     }
    // }
    SW1_state = digitalRead(SW1);
    SW2_state = digitalRead(SW2);
    if (SW1_state == 0 && M1_state == 0)
    {
        digitalWrite(M2_A, LOW);
        digitalWrite(M2_B, HIGH);
        ledcWrite(motor_PWM_channelB, 220);
        // detachInterrupt(SW1);
        delay(200);
        digitalWrite(M2_A, LOW);
        digitalWrite(M2_B, LOW);
        ledcWrite(motor_PWM_channelB, 0);
        Serial.println("SW1down");
        // pinMode(SW1, INPUT_PULLUP);
        // attachInterrupt(SW1, interruptServiceRoutinesw1, ONLOW);
        // SW1_state = true;
    }
    else if (SW2_state == 0 && M1_state == 0)
    {
        digitalWrite(M2_B, LOW);
        digitalWrite(M2_A, HIGH);
        // detachInterrupt(SW2);
        ledcWrite(motor_PWM_channelB, 220);
        delay(200);
        digitalWrite(M2_B, LOW);
        digitalWrite(M2_A, LOW);
        ledcWrite(motor_PWM_channelB, 0);
        Serial.println("SW2down");
        // pinMode(SW2, INPUT_PULLUP);
        // attachInterrupt(SW2, interruptServiceRoutinesw2, ONLOW);
        // SW2_state = true;
    }
}

/*

*/

void PWM_init()
{
    pinMode(M1_A, OUTPUT);
    digitalWrite(M1_A, LOW);
    pinMode(M1_B, OUTPUT);
    digitalWrite(M1_B, LOW);
    pinMode(M2_A, OUTPUT);
    digitalWrite(M2_A, LOW);
    pinMode(M2_B, OUTPUT);
    digitalWrite(M2_B, LOW);
    ledcAttachPin(M1_PWM, motor_PWM_channelA);
    ledcSetup(motor_PWM_channelA, motor_PWM_frq, motor_PWM_res);
    ledcWrite(motor_PWM_channelA, 0);
    ledcAttachPin(M2_PWM, motor_PWM_channelB);
    ledcSetup(motor_PWM_channelB, motor_PWM_frq, motor_PWM_res);
    ledcWrite(motor_PWM_channelB, 0);
}
void setdir_speedA(int dir, uint32_t speed)
{
    set_speedA(speed);
    set_dir(dir);
}

void set_speedA(uint32_t speed)
{
    if (speed != 0)
    {
        speed = speed * ((255.0 - pwma_vaule) / 100.0) + pwma_vaule;
        ledcWrite(motor_PWM_channelA, speed);
    }
    else
    {
        ledcWrite(motor_PWM_channelA, 0);
    }
    // Serial.println(ledcRead(0));
    // Serial.println(ledcRead(1));
}

void set_dir(int dir)
{
    if (dir == 1)
    {
        digitalWrite(M1_A, LOW);
        digitalWrite(M1_B, HIGH);
    }
    else if (dir == 0)
    {
        digitalWrite(M1_A, HIGH);
        digitalWrite(M1_B, LOW);
    }
}

void setdir_speedB(int dir, uint32_t speed)
{
    if (speed != 0)
    {
        speed = speed * ((255.0 - pwmb_vaule) / 100.0) + pwmb_vaule;
        ledcWrite(motor_PWM_channelB, speed);
    }
    else
    {
        ledcWrite(motor_PWM_channelB, 0);
    }
    if (dir == 1)
    {
        digitalWrite(M2_A, LOW);
        digitalWrite(M2_B, HIGH);
    }
    else if (dir == 0)
    {
        digitalWrite(M2_A, HIGH);
        digitalWrite(M2_B, LOW);
    }
}

void setsetp_speed(int dir, uint32_t setpspeed)
{
    set_dir(dir);
    switch (setpspeed)
    {
    case 1:
        set_speedA(PWM_SETP1);
    case 2:
        set_speedA(PWM_SETP2);
    case 3:
        set_speedA(PWM_SETP3);
    case 4:
        set_speedA(PWM_SETP4);
    case 5:
        set_speedA(PWM_SETP5);
    case 6:
        set_speedA(PWM_SETP6);
    case 7:
        set_speedA(PWM_SETP7);
    default:
        break;
    }
}

void HX_init()
{
    pinMode(HX_SCK, OUTPUT);
    pinMode(HX_DT, INPUT);
    ReadCount();
    delay(100);
    // for (int i = 0; i < 8; i++)
    //     count0 += ReadCount();
    // count0 /= 8;
    count0 = 180000;
    Serial.print("count0:");
    Serial.println(count0);
}

long ReadCount()
{
    pinMode(HX_SCK, OUTPUT);
    pinMode(HX_DT, INPUT);
    digitalWrite(HX_SCK, LOW);
    while (digitalRead(HX_DT))
        ;
    unsigned long count_temp = 0;
    for (int i = 0; i < 24; i++)
    {
        digitalWrite(HX_SCK, HIGH);
        count_temp <<= 1;
        digitalWrite(HX_SCK, LOW);
        if (digitalRead(HX_DT))
            count_temp |= 1;
    }
    digitalWrite(HX_SCK, HIGH);
    if (count_temp & 0x00800000)
        count_temp |= 0xFF000000;
    digitalWrite(HX_SCK, LOW);
    return (long)count_temp;
}

/*

*/

/*
    attachInterrupt(SS,interruptServiceRoutine,FALLING);
*/

void sw_init(void)
{
    pinMode(SW1, INPUT_PULLUP);
    pinMode(SW2, INPUT_PULLUP);
    // attachInterrupt(SW1, interruptServiceRoutinesw1, ONLOW);
    // attachInterrupt(SW2, interruptServiceRoutinesw2, ONLOW);
}

void interruptServiceRoutinesw1(void)
{
    digitalWrite(M2_A, LOW);
    digitalWrite(M2_B, LOW);
    SW1_state = true;
    detachInterrupt(SW1);
    //Serial.println("SW1down");
}
void interruptServiceRoutinesw2(void)
{
    digitalWrite(M2_A, LOW);
    digitalWrite(M2_B, LOW);
    SW2_state = true;
    detachInterrupt(SW2);
    //Serial.println("SW2down");
}

void sw3_init(void)
{
    pinMode(SW3, INPUT_PULLUP);
    // attachInterrupt(SW3, interruptServiceRoutinesw3, ONLOW);
}

void interruptServiceRoutinesw3(void)
{
    urgent_stop();
    SW3_state = false;
    //Serial.println("SW3down");
    delay(10);
}

void urgent_stop(void)
{
    digitalWrite(M2_A, LOW);
    digitalWrite(M2_B, LOW);
    ledcWrite(motor_PWM_channelA, 0);
    digitalWrite(M1_A, LOW);
    digitalWrite(M1_B, LOW);
    ledcWrite(motor_PWM_channelB, 0);
}

void ashcan_push()
{
    setdir_speedB(0, pushspeed);
}

void ashcan_pop()
{
    setdir_speedB(1, popspeed);
}
/*


*/
/*********************/
